#include "PIDControler.h"
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <iostream>


PIDControler::PIDControler(boost::function<double (void)> ImportFunc, boost::function<void (double)> ExportFunc, double &Consigne, double Min, double Max, double CoefP, double CoefI, double CoefD):
m_ImportFunc(ImportFunc),
m_ExportFunc(ExportFunc),
m_error(0.),
m_integral(0.),
m_differential(0.),
m_gain_dif(CoefD),
m_gain_int(CoefI),
m_gain_error(CoefP),
m_pidMax(Max),
m_pidMin(Min),
m_target(Consigne),
m_FirstTime(true),
consigne(0)
{
}

PIDControler::~PIDControler()
{
}

void PIDControler::Iteration()
{
	if(true == m_FirstTime)
	{
		gettimeofday(&lastTime, NULL);
		m_FirstTime = false;
	}
	else
	{
		// Calculate positional error
		double actual = m_ImportFunc();
		double error = m_target - actual;

		gettimeofday(&currentTime, NULL);

		double DeltatTime = (currentTime.tv_sec + currentTime.tv_usec * 0.000001) -
					  (lastTime.tv_sec + lastTime.tv_usec * 0.000001);

		lastTime = currentTime;

		m_dt = 1. / DeltatTime;

		// Calculate the differential between last error
		m_differential = (error - m_error) / m_dt;

		// Update integral with new error
		m_integral += (error * m_dt);

		// Save the last last error
		m_error = error;

		// Sum the raw PID components
		double rawpid =((error * m_gain_error)+(m_integral * m_gain_int)+(m_differential * m_gain_dif));

		consigne += rawpid;

		// Return clamped PID value
		if (consigne > m_pidMax)
			consigne = m_pidMax;
		else if (consigne < m_pidMin)
			consigne = m_pidMin;

		m_ExportFunc(consigne);

		//std:: cout << "actual : " << actual << " error: " << error << " consigne: " << consigne << std::endl;
	}
}
